
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_motors_interface.c
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gp_motors_matrix.h"

#include <parameter/param.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// Constructor
void MotorsMulticopter(MotorsMC_HandleTypeDef *pMotors, uint16_t loop_rate, uint16_t speed_hz)
{
    Motors((Motors_HandleTypeDef*)pMotors, loop_rate, speed_hz);

    // 更新参数
    MotorsMC_update_param(pMotors);
    
    pMotors->_lift_max = 1.0f;
    pMotors->_throttle_limit = 1.0f;

    // setup battery voltage filtering
    lpf_set_cutoff1(&(pMotors->_batt_voltage_filt),GP_MOTORS_BATT_VOLT_FILT_HZ);
    lpf_reset(&(pMotors->_batt_voltage_filt), 1.0f);

    // default throttle range
    pMotors->_throttle_radio_min = 1100;
    pMotors->_throttle_radio_max = 1900;
}

//更新参数
void MotorsMC_update_param(MotorsMC_HandleTypeDef *pMotors)
{
    if (pMotors->_yaw_headroom != PARAM_GET_INT16(MOTOR,MOT_YAW_HEADROOM)) {
        pMotors->_yaw_headroom = PARAM_GET_INT16(MOTOR,MOT_YAW_HEADROOM);
    }

    if (!math_flt_equal(pMotors->_thrust_curve_expo,PARAM_GET_FLOAT(MOTOR,MOT_THST_EXPO))) {
        pMotors->_thrust_curve_expo = PARAM_GET_FLOAT(MOTOR,MOT_THST_EXPO);
    }

    if (!math_flt_equal(pMotors->_slew_up_time,PARAM_GET_FLOAT(MOTOR,MOT_SLEW_UP_TIME))) {
        pMotors->_slew_up_time = PARAM_GET_FLOAT(MOTOR,MOT_SLEW_UP_TIME);
    }

    if (!math_flt_equal(pMotors->_slew_dn_time,PARAM_GET_FLOAT(MOTOR,MOT_SLEW_DN_TIME))) {
        pMotors->_slew_dn_time = PARAM_GET_FLOAT(MOTOR,MOT_SLEW_DN_TIME);
    }

    if (!math_flt_equal(pMotors->_safe_time,PARAM_GET_FLOAT(MOTOR,MOT_SAFE_TIME))) {
        pMotors->_safe_time = PARAM_GET_FLOAT(MOTOR,MOT_SAFE_TIME);
    }

    if (!math_flt_equal(pMotors->_spin_min,PARAM_GET_FLOAT(MOTOR,MOT_SPIN_MIN))) {
        pMotors->_spin_min = PARAM_GET_FLOAT(MOTOR,MOT_SPIN_MIN);
    }

    if (!math_flt_equal(pMotors->_spin_max,PARAM_GET_FLOAT(MOTOR,MOT_SPIN_MAX))) {
        pMotors->_spin_max = PARAM_GET_FLOAT(MOTOR,MOT_SPIN_MAX);
    }

    if (!math_flt_equal(pMotors->_spin_arm,PARAM_GET_FLOAT(MOTOR,MOT_SPIN_ARM))) {
        pMotors->_spin_arm = PARAM_GET_FLOAT(MOTOR,MOT_SPIN_ARM);
    }

    if (!math_flt_equal(pMotors->_batt_voltage_max,PARAM_GET_FLOAT(MOTOR,MOT_BAT_VOLT_MAX))) {
        pMotors->_batt_voltage_max = PARAM_GET_FLOAT(MOTOR,MOT_BAT_VOLT_MAX);
    }

    if (!math_flt_equal(pMotors->_batt_voltage_min,PARAM_GET_FLOAT(MOTOR,MOT_BAT_VOLT_MIN))) {
        pMotors->_batt_voltage_min = PARAM_GET_FLOAT(MOTOR,MOT_BAT_VOLT_MIN);
    }

    if (!math_flt_equal(pMotors->_batt_current_max,PARAM_GET_FLOAT(MOTOR,MOT_BAT_CURR_MAX))) {
        pMotors->_batt_current_max = PARAM_GET_FLOAT(MOTOR,MOT_BAT_CURR_MAX);
    }

    if (!math_flt_equal(pMotors->_batt_current_time_constant,PARAM_GET_FLOAT(MOTOR,MOT_BAT_CURR_TC))) {
        pMotors->_batt_current_time_constant = PARAM_GET_FLOAT(MOTOR,MOT_BAT_CURR_TC);
    }

    if (pMotors->_batt_idx != PARAM_GET_INT8(MOTOR,MOT_BAT_IDX)) {
        pMotors->_batt_idx = PARAM_GET_INT8(MOTOR,MOT_BAT_IDX);
    }

    if (pMotors->_pwm_min != PARAM_GET_INT16(MOTOR,MOT_PWM_MIN)) {
        pMotors->_pwm_min = PARAM_GET_INT16(MOTOR,MOT_PWM_MIN);
    }

    if (pMotors->_pwm_max != PARAM_GET_INT16(MOTOR,MOT_PWM_MAX)) {
        pMotors->_pwm_max = PARAM_GET_INT16(MOTOR,MOT_PWM_MAX);
    }

    if (!math_flt_equal(pMotors->_throttle_hover,PARAM_GET_FLOAT(MOTOR,MOT_THST_HOVER))) {
        pMotors->_throttle_hover = PARAM_GET_FLOAT(MOTOR,MOT_THST_HOVER);
    }

    if (pMotors->_throttle_hover_learn != PARAM_GET_INT8(MOTOR,MOT_HOVER_LEARN)) {
        pMotors->_throttle_hover_learn = PARAM_GET_INT8(MOTOR,MOT_HOVER_LEARN);
    }

    if (pMotors->_disarm_disable_pwm != PARAM_GET_INT8(MOTOR,MOT_SAFE_DISARM)) {
        pMotors->_disarm_disable_pwm = PARAM_GET_INT8(MOTOR,MOT_SAFE_DISARM);
    }

    if (!math_flt_equal(pMotors->_yaw_servo_angle_max_deg,PARAM_GET_FLOAT(MOTOR,MOT_YAW_SV_ANGLE))) {
        pMotors->_yaw_servo_angle_max_deg = PARAM_GET_FLOAT(MOTOR,MOT_YAW_SV_ANGLE);
    }

    if (!math_flt_equal(pMotors->_spool_up_time,PARAM_GET_FLOAT(MOTOR,MOT_SPOOL_TIME))) {
        pMotors->_spool_up_time = PARAM_GET_FLOAT(MOTOR,MOT_SPOOL_TIME);
    }

    if (!math_flt_equal(pMotors->_boost_scale,PARAM_GET_FLOAT(MOTOR,MOT_BOOST_SCALE))) {
        pMotors->_boost_scale = PARAM_GET_FLOAT(MOTOR,MOT_BOOST_SCALE);
    }
}

// set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
// also sets minimum and maximum pwm values that will be sent to the motors
void MotorsMC_set_throttle_range(MotorsMC_HandleTypeDef *pMotors, int16_t radio_min, int16_t radio_max)
{
    Motors_HandleTypeDef* motors = (Motors_HandleTypeDef*)pMotors;

    // sanity check
    if (radio_max <= radio_min) {
        return;
    }

    pMotors->_throttle_radio_min = radio_min;
    pMotors->_throttle_radio_max = radio_max;

    if (motors->_pwm_type >= PWM_TYPE_DSHOT150 && motors->_pwm_type <= PWM_TYPE_DSHOT1200) {
        // force PWM range for DShot ESCs
        PARAM_SET_INT16(MOTOR,MOT_PWM_MIN,1000);
        PARAM_SET_INT16(MOTOR,MOT_PWM_MAX,2000);
    }

    //TODO: 设置pwm驱动的输出范围
    
}

// run spool logic
void MotorsMC_output_logic(MotorsMC_HandleTypeDef *pMotors)
{
    Motors_HandleTypeDef* motors = (Motors_HandleTypeDef*)pMotors;
    float spin_up_armed_ratio = 0.0f;

    if (Motors_get_armed(motors)) {
        if (pMotors->_disarm_disable_pwm && (pMotors->_disarm_safe_timer < pMotors->_safe_time)) {
            pMotors->_disarm_safe_timer += 1.0f/motors->_loop_rate;
        } else {
            pMotors->_disarm_safe_timer = pMotors->_safe_time;
        }
    } else {
           pMotors->_disarm_safe_timer = 0.0f;
    }

    // force desired and current spool mode if disarmed or not interlocked
    if (!Motors_get_armed(motors) || !Motors_get_interlock(motors)) {
        motors->_spool_desired = MOTOR_DESIRED_SHUT_DOWN;
        motors->_spool_state = MOTOR_SHUT_DOWN;
    }

    if (pMotors->_spool_up_time < 0.05f) {
        // prevent float exception
        PARAM_SET_FLOAT(MOTOR,MOT_SPOOL_TIME,0.05f);
    }

    switch (motors->_spool_state) {
    case MOTOR_SHUT_DOWN:
        // Motors should be stationary.
        // Servos set to their trim values or in a test condition.

        // set limits flags
        motors->limit.roll = true;
        motors->limit.pitch = true;
        motors->limit.yaw = true;
        motors->limit.throttle_lower = true;
        motors->limit.throttle_upper = true;

        // make sure the motors are spooling in the correct direction
        if (motors->_spool_desired != MOTOR_DESIRED_SHUT_DOWN && pMotors->_disarm_safe_timer >= pMotors->_safe_time) {
            motors->_spool_state = MOTOR_GROUND_IDLE;
            break;
        }

        // set and increment ramp variables
        pMotors->_spin_up_ratio = 0.0f;
        pMotors->_throttle_thrust_max = 0.0f;

        // initialise motor failure variables
        motors->_thrust_boost = false;
        motors->_thrust_boost_ratio = 0.0f;
        break;

    case MOTOR_GROUND_IDLE: {
        // Motors should be stationary or at ground idle.
        // Servos should be moving to correct the current attitude.

        // set limits flags
        motors->limit.roll = true;
        motors->limit.pitch = true;
        motors->limit.yaw = true;
        motors->limit.throttle_lower = true;
        motors->limit.throttle_upper = true;

        // set and increment ramp variables
        float spool_step = 1.0f / (pMotors->_spool_up_time * motors->_loop_rate);
        switch (motors->_spool_desired) {
        case MOTOR_DESIRED_SHUT_DOWN:
            pMotors->_spin_up_ratio -= spool_step;
            // constrain ramp value and update mode
            if (pMotors->_spin_up_ratio <= 0.0f) {
                pMotors->_spin_up_ratio = 0.0f;
                motors->_spool_state = MOTOR_SHUT_DOWN;
            }
            break;

        case MOTOR_DESIRED_THROTTLE_UNLIMITED:
            pMotors->_spin_up_ratio += spool_step;
            // constrain ramp value and update mode
            if (pMotors->_spin_up_ratio >= 1.0f) {
                pMotors->_spin_up_ratio = 1.0f;
                motors->_spool_state = MOTOR_SPOOLING_UP;
            }
            break;

        case MOTOR_DESIRED_GROUND_IDLE:
            if (pMotors->_spin_min > 0.0f) {
                spin_up_armed_ratio = pMotors->_spin_arm / pMotors->_spin_min;
            }
            pMotors->_spin_up_ratio += math_constrain_float(spin_up_armed_ratio - pMotors->_spin_up_ratio, -spool_step, spool_step);
            break;
        }
        pMotors->_throttle_thrust_max = 0.0f;

        // initialise motor failure variables
        motors->_thrust_boost = false;
        motors->_thrust_boost_ratio = 0.0f;
        break;
    }
    case MOTOR_SPOOLING_UP:
        // Maximum throttle should move from minimum to maximum.
        // Servos should exhibit normal flight behavior.

        // initialize limits flags
        motors->limit.roll = false;
        motors->limit.pitch = false;
        motors->limit.yaw = false;
        motors->limit.throttle_lower = false;
        motors->limit.throttle_upper = false;

        // make sure the motors are spooling in the correct direction
        if (motors->_spool_desired != MOTOR_DESIRED_THROTTLE_UNLIMITED) {
            motors->_spool_state = MOTOR_SPOOLING_DOWN;
            break;
        }

        // set and increment ramp variables
        pMotors->_spin_up_ratio = 1.0f;
        pMotors->_throttle_thrust_max += 1.0f / (pMotors->_spool_up_time * motors->_loop_rate);

        // constrain ramp value and update mode
        if (pMotors->_throttle_thrust_max >= MIN(Motors_get_throttle(motors), MotorsMC_get_current_limit_max_throttle(pMotors))) {
            pMotors->_throttle_thrust_max = MotorsMC_get_current_limit_max_throttle(pMotors);
            motors->_spool_state = MOTOR_THROTTLE_UNLIMITED;
        } else if (pMotors->_throttle_thrust_max < 0.0f) {
            pMotors->_throttle_thrust_max = 0.0f;
        }

        // initialise motor failure variables
        motors->_thrust_boost = false;
        motors->_thrust_boost_ratio = MAX(0.0f, motors->_thrust_boost_ratio - 1.0f / (pMotors->_spool_up_time * motors->_loop_rate));
        break;

    case MOTOR_THROTTLE_UNLIMITED:
        // Throttle should exhibit normal flight behavior.
        // Servos should exhibit normal flight behavior.

        // initialize limits flags
        motors->limit.roll = false;
        motors->limit.pitch = false;
        motors->limit.yaw = false;
        motors->limit.throttle_lower = false;
        motors->limit.throttle_upper = false;

        // make sure the motors are spooling in the correct direction
        if (motors->_spool_desired != MOTOR_DESIRED_THROTTLE_UNLIMITED) {
            motors->_spool_state = MOTOR_SPOOLING_DOWN;
            break;
        }

        // set and increment ramp variables
        pMotors->_spin_up_ratio = 1.0f;
        pMotors->_throttle_thrust_max = MotorsMC_get_current_limit_max_throttle(pMotors);

        if (motors->_thrust_boost && !motors->_thrust_balanced) {
            motors->_thrust_boost_ratio = MIN(1.0f, motors->_thrust_boost_ratio + 1.0f / (pMotors->_spool_up_time * motors->_loop_rate));
        } else {
            motors->_thrust_boost_ratio = MAX(0.0f, motors->_thrust_boost_ratio - 1.0f / (pMotors->_spool_up_time * motors->_loop_rate));
        }
        break;

    case MOTOR_SPOOLING_DOWN:
        // Maximum throttle should move from maximum to minimum.
        // Servos should exhibit normal flight behavior.

        // initialize limits flags
        motors->limit.roll = false;
        motors->limit.pitch = false;
        motors->limit.yaw = false;
        motors->limit.throttle_lower = false;
        motors->limit.throttle_upper = false;

        // make sure the motors are spooling in the correct direction
        if (motors->_spool_desired == MOTOR_DESIRED_THROTTLE_UNLIMITED) {
            motors->_spool_state = MOTOR_SPOOLING_UP;
            break;
        }

        // set and increment ramp variables
        pMotors->_spin_up_ratio = 1.0f;
        pMotors->_throttle_thrust_max -= 1.0f / (pMotors->_spool_up_time * motors->_loop_rate);

        // constrain ramp value and update mode
        if (pMotors->_throttle_thrust_max <= 0.0f) {
            pMotors->_throttle_thrust_max = 0.0f;
        }
        if (pMotors->_throttle_thrust_max >= MotorsMC_get_current_limit_max_throttle(pMotors)) {
            pMotors->_throttle_thrust_max = MotorsMC_get_current_limit_max_throttle(pMotors);
        } else if (math_flt_zero(pMotors->_throttle_thrust_max)) {
            motors->_spool_state = MOTOR_GROUND_IDLE;
        }

        motors->_thrust_boost_ratio = MAX(0.0f, motors->_thrust_boost_ratio - 1.0f / (pMotors->_spool_up_time * motors->_loop_rate));
        break;
    }
}

// output roll/pitch/yaw/thrust
void MotorsMC_output_rpyt(MotorsMC_HandleTypeDef *pMotors)
{

}

// save parameters as part of disarming
void MotorsMC_save_params_on_disarm(MotorsMC_HandleTypeDef *pMotors)
{
    // save hover throttle
    if (pMotors->_throttle_hover_learn == HOVER_LEARN_AND_SAVE) {
        PARAM_SET_FLOAT(MOTOR,MOT_THST_HOVER,pMotors->_throttle_hover);
        param_save();
    }
}

// update estimated throttle required to hover
void MotorsMC_update_throttle_hover(MotorsMC_HandleTypeDef *pMotors, float dt)
{
    Motors_HandleTypeDef* motors = (Motors_HandleTypeDef*)pMotors;

    if (pMotors->_throttle_hover_learn != HOVER_LEARN_DISABLED) {
        // we have chosen to constrain the hover throttle to be within the range reachable by the third order expo polynomial.
        pMotors->_throttle_hover = math_constrain_float(pMotors->_throttle_hover + (dt / (dt + GP_MOTORS_THST_HOVER_TC)) * (Motors_get_throttle(motors) - pMotors->_throttle_hover), GP_MOTORS_THST_HOVER_MIN, GP_MOTORS_THST_HOVER_MAX);
        PARAM_SET_FLOAT(MOTOR,MOT_THST_HOVER,pMotors->_throttle_hover);
    }
}

/*------------------------------------test------------------------------------*/


